Parallel Parking Problem

Anshul Lodha 110422226

I have chosen to design a control system for self-parking car. The problem statement is formulated as follows.

Problem statement: The problem is divided into sub-tasks as

1. You are given a lane with multiple cars parked in parallel and the car moves through the lane to determine a place to park the car using sensors to determine the distance between two cars.
2. Once a feasible space has been identified the parallel parking algorithm is started, Backward or Forward parallel parking schema can be used depending on amount of space available; for simplicity we assume a backward parallel parking for all cases irrespective of the space available.

Assumptions:

1. For the automatic parallel parking system, normally ultra-sonic sensors or infra-red sensors are used to measure the parking space. These sensors can not measure the exact depth of the space if there is no wall and generally we park the vehicle in a line with neighboring vehicles. Therefore we assume in the following sections that the depth of the space is equal to the width of the vehicle.
2. Since a parking controller operates the vehicle at low speeds, it is common and reasonable to assume no tire slip.
3. Maximum steering angle for rotaional motion
4. The vehicle is represented by a kinematic bicycle model

Approach

Following steps will be involved,

• Generating Desired Trajectory: Desired trajectory and control are genrated by implementing the mathematical model in GPOPs solver. Actuator and steer constrains are taken into account.
• Controller Design: Based on the desired trajectories a high-gain controller for the system has been designed. As, within a given neighborhood of states and control, the system behaves like a linear system. Therefore, if we compute a base trajecotry that roughly follows the dynamics of the system and gives optimal control under some simplified assumptions, we can construct a trajectory following controller by linearing the system about the desired trajectory and using techniques from linear control to drive errors to zero.
• Using Extended Kalman filter to remove unwanted noise

Mathematical Model

The vehicle model has a three dimensional configuration space and three additional dynamic states , Here denotes the absolute speed of the vehicle and the front wheel steering angle.

All the corresponding parameters of the CLMR(Car Like Moving Robot) depicted in Fig. 1 are defined as follows.

position of the front wheel center of CLMR;

position of the rear wheel center of CLMR;

orientation of the steering-wheels with respect to the frame of CLMR;

angle between vehicle frame orientation and X-axis;

wheel-base of CLMR;

Velocity of the CLMR

As no slipping condition is assumed, we have

The front dof the CLMR is fixed relative to the rear, thus

Differentiating w.r.t time gives,

Substituing in (1) gives

From Fig. 1, we have

Substituting we can derive,

Equations (5) and (6) are the kinematic equations of CLMR with respect to the axle center of the front wheels. Using (3) ,(5) and (6) after some algebra, the kinematics of with respect to the axle center of the rear wheels will be described as

...(6)

This mathematical model would work however it will have rate of change of steering angle will not be controlled. In order to have minimum maneuvering we would add one more state to the system such that obeys first order dynamics.

Actuator dynamics refers to the characteristic that most actuators do not have an infinite bandwidth, i.e. they have a response delay and cannot apply a large control signal starting from rest. In such cases, it is possible to append control to the state-space as a first order system, with commanded control being the new control input. Say the actuator dynamics are given by,

where is the commanded control input, and is the parameter characterizing the actuator dynamics.

Combining equations (6),(7) and (8), we can have full control over the system and these equations can be used to generate the next backward state position of the vehicle when the present state and control input are given. and are the control parameters for this system.

For measurement we will have two measurements, one for x-coordinate and other for y-coordinate. These measurements in real world can be obtained using GPS

Generate Desired Trajectory

Trajectory generation is done in this example using GPOPs II solver. Actuator dynamics and constraints have been imposed while modeling the problem. The above derived equations of motion were used to generate a path for a real environment problem.

Thus we can see the path generated for the problem, however this does not imply that the control signal ouput given by GPOPs II will trace the same path when actually applied to EOM because in this method dynamics are only satisfied at collocation points.

Also this control signal does not accoount for any unexpected perturbations, thus a controller is required to drive the states to the desired path by applying control which accounts for all external factors.

Below is solution generated by GPOPs

Controllability

System without taking actuator dynamics into account can be represented as

Compute lie bracket of [steer,drive], defined as Wriggle

This is a new direction

Now, Compute lie bracket of [wriggle,drive]

Solving gives additional direction

Thus this system is controllable

Controller

Now that need of a controller is eastablished, we will implement a high gain controller.

It is difficult to compute optimal control policies for most nonlinear systems. However, within a given neighborhood of states and control, the system behaves like a linear system. Therefore, if we compute a base trajecotry that roughly follows the dynamics of the system and gives optimal control under some simplified assumptions, we can construct a trajectory following controller by linearing the system about the desired trajectory and using techniques from linear control to drive errors to zero.

The additional linearized control is also necessary to compensate for any unexpected perturbations that may be present due to external factors not accounted for in the controller design.

The linearized form of the system can be obtained as:

Defining and , gives

The linearized system can be obatined by taking Taylor series expansion about gives,

Ignoring the higher order terms gives,

Linearized system:

where

All the techniques of linear control system can be applied to the system described above. In this case we will use Full-state feedback for a discrete system. Note the expression above depends on both the desired state and control variables and the final control to be applied to the system will be,

Gain can be calculated by setting the poles to the negative side so that the control is stable, so this parameter can be varied to minimize the error.

main_control

Lyapunov Method

As we saw before, linearization can be used to design controllers for local region around some operating point. However, the properties of linearized system need not apply to the nonlinear system

kinematics:

We define a Lyapunov as,

Taking derivative gives,

Substituting states from Eqn. (6) and solving

Therefore choosing

This gives the Lyapunov derivative as

For this kinematic system system, let , therefore

Therefore

Dynamics Control

Taking Derivative and substituting gives,

Therefore choosing

here is the parameter characterizing the actuator dynamics(defined in (8)).

Therefore choosing these values for control gives a stable control system that drives states to desired values.

For linearised system

Lyapunov method can also be applied to the linearized system defined in the controller part of this document to demonstrate the stability of the system at collocation points, it is not applied here as Lyapunov methods are not typically applied for linear systems because the conditions for the states to go to zero can be verified using Eign values of

Observer

Discretizing the system

Seperating the terms

Implementing the Extended kalman filter for this system.

Extended_kalman;

Seperation Principle

This observer model can be used with the controller model using seperation principle

As it can be seen there is a large error between the estimates and actual values especially at the start. Therefore, it is possible that a control scheme based on observer estimates can lead to poorer performance of the entire system

Separation princple allows for the design of the controller and oberver separately, and lets us use these estimates for control. Separation principle states that under some assumptions the problem of designing an optimal feedback controller for a stochastic system can be solved by designing an observer for the state of the system, which feeds into a deterministic controller for the system

Initial errors behaviour can be seen on the below model.

Simulation Expirement:

Simulation expirements were carried out in four environments, one of them being the classic garage parking problem. Others are various lane size and distances

Parameter uncertainies have been modelled in the observer design and are taken care of, so variations in parameters wont affect the algorithm to a significant extent

This controller will fail to achieve success when the sensors are too much noisy and the reading are hampered significantly. Controller will also likely fail on initial and final locations which are too close, for e.g a location which has relative distance less than the track of the vehicle and orientation change around degrees in such cases it is basically not practicle to move a CLMR.